#include "mex.h"
#include "matrix.h"
#include <math.h>
#include <limits>
#include <process.h>    /* _beginthread, _endthread */
#include <windows.h>	/* WaitForMultipleObjects */
#include <vector>
#include <time.h>

#undef max //<windows.h> has a macro named max which we need to undefine so we can use the max in limits

/*
 Copyright 2011, Jeff Craig and Michael Jensen.
  
 Threaded 3D Gamma is distributed under the terms of the Lesser GNU Public License. 
 
     This version of Threaded 3D Gamma is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation, either version 3 of the License, or
     (at your option) any later version.
 
 Threaded 3D Gamma is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
 without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 See the GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with Threaded 3D Gamma.  If not, see <http://www.gnu.org/licenses/>.
*/


//Use the following matlab command to compile.
//mex Threaded3DGammamex.cpp
//

double* gamma; // Global. Array matching size of refference dose and evaluation dose, contains gamma values
double* gamma_xdist; // Global. Array matching size of refference dose and evaluation dose, contains x distance to lowest gamma
double* gamma_ydist; // Global. Array matching size of refference dose and evaluation dose, contains y distance to lowest gamma
double* gamma_zdist; // Global. Array matching size of refference dose and evaluation dose, contains z distance to lowest gamma

struct ThreadInfo{
//refdose - Array containing the reference dose
//refX - Array matching refDose. Each value corresponds to the X position of the same index in refDose
//refY - Array matching refDose. Each value corresponds to the y position of the same index in refDose
//refStart - Starting index for refDose
//refEnd - Ending index for refDose
//evldose - Array containing the evaluation dose
//evlX - Array matching evalDose. Each value corresponds to the X position of the same index in evalDose
//evlY - Array matching evalDose. Each value corresponds to the y position of the same index in evalDose
//evlLen - Length of evalDose
//DoseDiff - User chosen dose difference
//DTA - User chosen dose to agreement
	double* refdose;
	double* refX;
	double* refY;
    double* refZ;
	int refStart;
	int refEnd;
	double* evldose;
	double* evlX;
	double* evlY;
    double* evlZ;
	int evlLen;
	double DoseDiff;
	double DTA;
};


unsigned int __stdcall threadgamma(void* data)
{
//unsigned int __stdcall threadgamma(void* data)
//data - instance of ThreadInfo
//
//Method called by each thread
//
//Jeff Craig
//Dec 2010
  int i,j;
  double dose_diff,dist_x,dist_y,dist_z,r_sqrd,term1,term2,gamma_temp,gamma_hold,gamma_holdx,gamma_holdy,gamma_holdz;
  ThreadInfo *threadData = (ThreadInfo *)data;

  //Precalculating these since they never change.
  double DoseDiff = threadData->DoseDiff*threadData->DoseDiff;
  double DTA = threadData->DTA*threadData->DTA;

  clock_t init,final;
  init = clock();
  //loop through ref points
  for(i = threadData->refStart; i <= threadData->refEnd; i++) {
      //reset gamma_hold to maximum
      gamma_hold = std::numeric_limits<double>::max( );
      gamma_holdx = -1;
      gamma_holdy = -1;
      gamma_holdz = -1;
      
      //loop through all eval points
      for(j = 0; j < threadData->evlLen; j++) {
          //Dose Difference
          dose_diff = threadData->refdose[i] - threadData->evldose[j];
          //Distance
          dist_x = threadData->refX[i] - threadData->evlX[j];
          dist_y = threadData->refY[i] - threadData->evlY[j];
          dist_z = threadData->refZ[i] - threadData->evlZ[j];
          r_sqrd = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z;

          //Compute Gamma
          term1 = dose_diff * dose_diff / DoseDiff;
          term2 = r_sqrd / DTA;
          gamma_temp = sqrt(term1 + term2);
          
          if(gamma_temp < gamma_hold) {
              gamma_hold = gamma_temp;
              gamma_holdx = dist_x;
              gamma_holdy = dist_y;
              gamma_holdz = dist_z;
          }
      }
	  //No need to lock global resource since every thread will be accessing different indices.
      gamma[i] = gamma_hold;
      gamma_xdist[i] = gamma_holdx;
      gamma_ydist[i] = gamma_holdy;
      gamma_zdist[i] = gamma_holdz;
  }
  final = clock()-init;
  mexPrintf("thread start index  %i\n",threadData->refStart);
  mexPrintf("thread end index  %i\n",threadData->refEnd);
  mexPrintf("thread time  %f\n",(double)final / ((double)CLOCKS_PER_SEC));

  return 0;
}

//Print out the dimensions of the array
void printDimensions(const mxArray *array,const char *name)
{
    const mwSize *dim_array;
    mwSize num_dim;
    dim_array = mxGetDimensions(array);
    num_dim = mxGetNumberOfDimensions(array);
    
    mexPrintf(name);
    for(int i = 0; i < num_dim; ++i)
    {
        if(0 != i)
            mexPrintf("x");
        mexPrintf(" %i ", dim_array[i]);
    }
    mexPrintf("\n");
}

bool compareDimensions(const mxArray *array1, const mxArray *array2)
{
    const mwSize *dim_array1, *dim_array2;
    dim_array1 = mxGetDimensions(array1);
    dim_array2 = mxGetDimensions(array2);
    
    mwSize num_dim1, num_dim2;
    num_dim1 = mxGetNumberOfDimensions(array1);
    num_dim2 = mxGetNumberOfDimensions(array2);
    
    if (num_dim1 != num_dim2)
        return false;
    
    for (int i = 0; i < num_dim1; ++i)
    {
        if(dim_array1[i] != dim_array2[i])
            return false;
    }
    
    return true;
}

/*
 * This function performs the core gamma calculation, using loops 
 * Inputs
 *  1 RefDose
 *  2 EvalDose
 *  3 RefDose X Coordinates
 *  4 RefDose Y Coordinates
 *  5 RefDose Z Coordinates
 *  6 EvalDose X Coordinates
 *  7 EvalDose Y Coordinates
 *  8 EvalDose z Coordinates
 *  9 DoseDiff 
 *  10 DTA
 *  11 Number of Threads
 * Outputs
 *  1 Gamma Function matrix
*/

void mexFunction(int nlhs, mxArray *plhs[],
    int nrhs, const mxArray *prhs[]) {
  
  const mwSize *dim_ref;
  const mwSize *dim_evl;

  //Get the dimensions of the Reference Dose arrays
  dim_ref = mxGetDimensions(prhs[0]);
  printDimensions(prhs[0],"Ref Dose");
  mwSize num_dim_ref;
  num_dim_ref = mxGetNumberOfDimensions(prhs[0]);
  //mexPrintf("Ref Dose %i x %i x %i \n",dim_ref[0],dim_ref[1],dim_ref[2]);
   
  //Get the dimensions of the Evaluation Dose arrays
  dim_evl = mxGetDimensions(prhs[1]);
  printDimensions(prhs[1],"Evl Dose");
  mwSize num_dim_evl;
  num_dim_evl = mxGetNumberOfDimensions(prhs[1]);
  //mexPrintf("Evl Dose %i x %i x %i \n",dim_evl[0],dim_evl[1],dim_evl[2]);
  
  const mwSize *dim_tmp;
  //Get the dimensions of the Dimension Arrays
  //Ref Data
  dim_tmp = mxGetDimensions(prhs[2]);
  printDimensions(prhs[2],"RefX Dose");
  if (compareDimensions(prhs[0],prhs[2]) == false)
      mexErrMsgTxt("Ref dose and Ref dose X coordinates do not match");
  //mexPrintf("RefX Dose %i x %i x %i \n",dim_tmp[0],dim_tmp[1],dim_tmp[2]);
  
  dim_tmp = mxGetDimensions(prhs[3]);
  printDimensions(prhs[3],"RefY Dose");
  if (compareDimensions(prhs[0],prhs[3]) == false)
      mexErrMsgTxt("Ref dose and Ref dose Y coordinates do not match");
  //mexPrintf("RefY Dose %i x %i x %i \n",dim_tmp[0],dim_tmp[1],dim_tmp[2]);
  
  dim_tmp = mxGetDimensions(prhs[4]);
  printDimensions(prhs[4],"RefZ Dose");
  if (compareDimensions(prhs[0],prhs[4]) == false)
      mexErrMsgTxt("Ref dose and Ref dose Z coordinates do not match");
  //mexPrintf("RefZ Dose %i x %i x %i \n",dim_tmp[0],dim_tmp[1],dim_tmp[2]);
  
  //Eval Data
  dim_tmp = mxGetDimensions(prhs[5]);
  printDimensions(prhs[5],"EvalX Dose");
  if (compareDimensions(prhs[1],prhs[5]) == false)
      mexErrMsgTxt("Eval dose and Eval dose X coordinates do not match");
  //mexPrintf("EvalX Dose %i x %i x %i \n",dim_tmp[0],dim_tmp[1],dim_tmp[2]);
  
  dim_tmp = mxGetDimensions(prhs[6]);
  printDimensions(prhs[6],"EvlY Dose");
  if (compareDimensions(prhs[1],prhs[6]) == false)
      mexErrMsgTxt("Eval dose and Eval dose Y coordinates do not match");
  //mexPrintf("EvalY Dose %i x %i x %i \n",dim_tmp[0],dim_tmp[1],dim_tmp[2]);
  
  dim_tmp = mxGetDimensions(prhs[7]);
  printDimensions(prhs[7],"EvlZ Dose");
  if (compareDimensions(prhs[1],prhs[7]) == false)
      mexErrMsgTxt("Eval dose and Eval dose Z coordinates do not match");
  //mexPrintf("EvalZ Dose %i x %i x %i \n",dim_tmp[0],dim_tmp[1],dim_tmp[2]);

  
  //Create array to be the same size as the reference dose array.
  //This will be where we store our gamma values to be returned.
  plhs[0] = mxCreateNumericArray(num_dim_ref, dim_ref ,mxDOUBLE_CLASS, mxREAL);
  gamma = mxGetPr(plhs[0]);
  
  //Displacement of gamma values.
  plhs[1] = mxCreateNumericArray(num_dim_ref, dim_ref ,mxDOUBLE_CLASS, mxREAL);
  gamma_xdist = mxGetPr(plhs[1]);
  plhs[2] = mxCreateNumericArray(num_dim_ref, dim_ref ,mxDOUBLE_CLASS, mxREAL);
  gamma_ydist = mxGetPr(plhs[2]);
  plhs[3] = mxCreateNumericArray(num_dim_ref, dim_ref ,mxDOUBLE_CLASS, mxREAL);
  gamma_zdist = mxGetPr(plhs[3]);
  
  // determine number of threads
  int nThreads = *mxGetPr(prhs[10]);
  mexPrintf("nThreads %i \n",nThreads);
  // determine the indices range per thread
  int nIndicesPerThread = 1;
  for(int i = 0; i < num_dim_ref; ++i)
      nIndicesPerThread = nIndicesPerThread * dim_ref[i];
  
  nIndicesPerThread = nIndicesPerThread / nThreads;
  

  std::vector<ThreadInfo> thrdInfo(nThreads);
  
  thrdInfo[0].refdose = mxGetPr(prhs[0]);
  thrdInfo[0].evldose = mxGetPr(prhs[1]);
  thrdInfo[0].refX = mxGetPr(prhs[2]);
  thrdInfo[0].refY = mxGetPr(prhs[3]);
  thrdInfo[0].refZ = mxGetPr(prhs[4]);
  thrdInfo[0].refStart = 0;
  thrdInfo[0].refEnd = nIndicesPerThread-1;
  thrdInfo[0].evlX = mxGetPr(prhs[5]);
  thrdInfo[0].evlY = mxGetPr(prhs[6]);
  thrdInfo[0].evlZ = mxGetPr(prhs[7]);
  thrdInfo[0].evlLen = 1;
  for(int i = 0; i < num_dim_evl; ++i)
    thrdInfo[0].evlLen = thrdInfo[0].evlLen * dim_evl[i];
  
  thrdInfo[0].DoseDiff = *mxGetPr(prhs[8]);
  thrdInfo[0].DTA = *mxGetPr(prhs[9]);

  for (size_t thrdNum = 1; thrdNum<nThreads; thrdNum++)
  {
	  thrdInfo[thrdNum] = thrdInfo[thrdNum-1];
	  thrdInfo[thrdNum].refStart += nIndicesPerThread;
	  thrdInfo[thrdNum].refEnd += nIndicesPerThread;
  }
  thrdInfo[nThreads-1].refEnd = 1;
  for(int i = 0; i < num_dim_ref; ++i)
    thrdInfo[nThreads-1].refEnd = thrdInfo[nThreads-1].refEnd * dim_ref[i];
  
  thrdInfo[nThreads-1].refEnd = thrdInfo[nThreads-1].refEnd - 1; // Incase number of indices / number of threads = odd number this will fix the last thread to hit the last index


  mexPrintf("Beginning Loop... Go have a coffee\n");
  std::vector<HANDLE> hThreads(nThreads);
  for (size_t thrdNum = 0; thrdNum<nThreads; thrdNum++)
  {
	  hThreads[thrdNum] = (HANDLE)_beginthreadex(NULL, 0, &threadgamma, &thrdInfo[thrdNum], 0, NULL);
  }

  WaitForMultipleObjects(static_cast<DWORD>(nThreads), &hThreads[0], TRUE, INFINITE);

}
